/*
 * BFPosition2d.java
 *
 * Created on 14 de diciembre de 2007, 02:42 AM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Robot;
import Util.Pose2d;
import javaclient2.*;
import javaclient2.structures.PlayerConstants;
import javaclient2.structures.PlayerPose;

/**
 *
 * @author alejo
 */
public class BFPosition2d extends Position2d{
    
    //colors used for identification
    private int baseColor;
    private int deviceColor;

    //initial pose of device
    private Pose2d initPos;
    private boolean isDebugging = false;
    
    //blobfinder interface
    private BlobfinderInterface blobfinder = null;
    
    
    /** Creates a new instance of BFPosition2d */
    public BFPosition2d(Robot robot, String name, int index,int baseColor, int deviceColor) {
        
        super(robot, name, index);
        this.baseColor = baseColor;
        this.deviceColor = deviceColor;
        
         blobfinder=robot.getConnection().requestInterfaceBlobfinder(deviceIndex, PlayerConstants.PLAYER_OPEN_MODE);

        if(blobfinder!=null)
            System.out.println("Interface Blobfinder:"+
                                Integer.toString(deviceIndex)+
                                    " of Robot:"+
                                        Integer.toString(robot.getId())+" Ok");
        else{
            System.out.println("Interface Blobfinder:"+
                                Integer.toString(deviceIndex)+
                                    " of Robot:"+
                                        Integer.toString(robot.getId())+" Fail");
            return;
        }

        this.setPlayerDevice(blobfinder);

        data=new Pose2d(0f,0f,0f);
        initPos = new Pose2d(0f,0f,0f);

    }
    
     public void setInitialPosition(Pose2d initial){
        //data.copy(initial);
        initial.copy(initPos);
     }
     
     
     /**
     *
     * Define si se puede leer o no
     */
    public boolean isReady()
    {
       return blobfinder.isDataReady();
    }
    
    
    public boolean update(){
        if(isReady()){
            int blobNumber = blobfinder.getData().getBlobs_count();
            int  red = getColorRed(blobfinder.getData().getBlobs()[0].getColor());
            int blue= getColorBlue(blobfinder.getData().getBlobs()[0].getColor());
            int  green = getColorGreen(blobfinder.getData().getBlobs()[0].getColor());
            System.out.print("Number of blobs: ");
            System.out.println(blobNumber);  
            
            for(int i=0; i<blobNumber;i++)
            {
                System.out.print("Id"); 
                System.out.print(i);
                System.out.print(": ");
                System.out.print(blobfinder.getData().getBlobs()[i].getArea());
                System.out.print(" Color: ");
                System.out.println(blobfinder.getData().getBlobs()[i].getColor());
                
            }
            
            
            return true;
        }
        else
            return false;
           
    
    }
    
    public int getColorRed(int color){
        return (color & 0x00FF0000)>>16;
    
    }
    
    public int getColorGreen(int color){
        return (color & 0x0000FF00)>>8;
    
    }
    
    public int getColorBlue(int color){
        return (color & 0x000000FF);
    
    }
    
    public void setSpeed(float val1, float val2){
        //Dont do anything here, this device acts only as a position sensor
        //not as an actuator!!!!!!
    }
    
    //public PlayerBlobfinderBlob[] get
}
